#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <geometry_msgs/msg/pose.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

void scale_trajectory_speed(moveit::planning_interface::MoveGroupInterface::Plan &plan, double scale)
{
    int n_joints = plan.trajectory_.joint_trajectory.joint_names.size();

    for (size_t i = 0; i < plan.trajectory_.joint_trajectory.points.size(); i++)
    {
        double time_in_seconds = plan.trajectory_.joint_trajectory.points[i].time_from_start.sec +
                                 plan.trajectory_.joint_trajectory.points[i].time_from_start.nanosec / 1e9;
        time_in_seconds /= scale;

        plan.trajectory_.joint_trajectory.points[i].time_from_start.sec = static_cast<int32_t>(time_in_seconds);
        plan.trajectory_.joint_trajectory.points[i].time_from_start.nanosec =
            static_cast<uint32_t>((time_in_seconds - plan.trajectory_.joint_trajectory.points[i].time_from_start.sec) * 1e9);

        for (int j = 0; j < n_joints; j++)
        {
            plan.trajectory_.joint_trajectory.points[i].velocities[j] *= scale;
            plan.trajectory_.joint_trajectory.points[i].accelerations[j] *= scale * scale;
        }
    }
}

bool returnToMiddle(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger) {
    move_group.setNamedTarget("middle");
    moveit::planning_interface::MoveGroupInterface::Plan home_plan;
    bool success = (move_group.plan(home_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    if (success) {
        move_group.execute(home_plan);
        RCLCPP_INFO(logger, "回到middle位置.");
        return true;
    } else {
        RCLCPP_ERROR(logger, "Failed to return to middle position.");
        return false;
    }
}

bool executeCartesianPath1(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger) {
    if (!returnToMiddle(move_group, logger)) {
        RCLCPP_ERROR(logger, "Failed to return to middle position before executing Cartesian path.");
        return false;
    }
    RCLCPP_INFO(logger, "Starting Cartesian path execution");

/* #region  */
    std::vector<geometry_msgs::msg::Pose> waypoints;
    geometry_msgs::msg::Pose pose1;
    pose1.position.x = 0.1038815445000545;
    pose1.position.y = 0.05599530875132761;
    pose1.position.z = 0.287593164792294;
    pose1.orientation.x = -7.443283672575919e-06;
    pose1.orientation.y = 7.57226530269504e-05;
    pose1.orientation.z = 0.12184966603307737;
    pose1.orientation.w = 0.9925485646044253;
    waypoints.push_back(pose1);

    geometry_msgs::msg::Pose pose2;
    pose2.position.x = 0.08365577107241515;
    pose2.position.y = 0.1086784487646846;
    pose2.position.z = 0.2876367813618629;
    pose2.orientation.x = 1.760144636797022e-06;
    pose2.orientation.y = 5.322223642645702e-07;
    pose2.orientation.z = 0.24192347154193786;
    pose2.orientation.w = 0.970295333346356;
    waypoints.push_back(pose2);

    geometry_msgs::msg::Pose pose3;
    pose3.position.x = -0.024580082964917316;
    pose3.position.y = 0.05115885226571315;
    pose3.position.z = 0.14427007528930358;
    pose3.orientation.x = -0.09252984740619437;
    pose3.orientation.y = 0.3713226058418401;
    pose3.orientation.z = 0.22355615291928813;
    pose3.orientation.w = 0.8964264589032053;
    waypoints.push_back(pose3);

    geometry_msgs::msg::Pose pose4;
    pose4.position.x = 0.0059124806471319985;
    pose4.position.y = 0.06736960543726173;
    pose4.position.z = 0.0851790937420965;
    pose4.orientation.x = -0.0787478196797887;
    pose4.orientation.y = 0.3158430604116397;
    pose4.orientation.z = 0.22880413581912243;
    pose4.orientation.w = 0.9174369784990976;
    waypoints.push_back(pose4);

    geometry_msgs::msg::Pose pose5;
    pose5.position.x = 0.014876747557903927;
    pose5.position.y = 0.07209511922660344;
    pose5.position.z = 0.069480923286174;
    pose5.orientation.x = -0.08464301863551622;
    pose5.orientation.y = 0.33968405068949703;
    pose5.orientation.z = 0.22656194017166972;
    pose5.orientation.w = 0.9089114326319655;
    waypoints.push_back(pose5);

    geometry_msgs::msg::Pose pose6;
    pose6.position.x = 0.032490128976992244;
    pose6.position.y = 0.08148829798108001;
    pose6.position.z = 0.08403494444521113;
    pose6.orientation.x = -0.0686576561182485;
    pose6.orientation.y = 0.275514230486597;
    pose6.orientation.z = 0.2319904963985905;
    pose6.orientation.w = 0.9303539351432134;
    waypoints.push_back(pose6);

    geometry_msgs::msg::Pose pose7;
    pose7.position.x = 0.044606693998343105;
    pose7.position.y = 0.08792626579373217;
    pose7.position.z = 0.08060679535604816;
    pose7.orientation.x = -0.06055302337925643;
    pose7.orientation.y = 0.24294582246096078;
    pose7.orientation.z = 0.23423143748854183;
    pose7.orientation.w = 0.9393861252969676;
    waypoints.push_back(pose7);

    geometry_msgs::msg::Pose pose8;
    pose8.position.x = 0.08050738757562798;
    pose8.position.y = 0.10705084955998226;
    pose8.position.z = 0.10661040040187303;
    pose8.orientation.x = -0.03783271841333385;
    pose8.orientation.y = 0.1517922939389349;
    pose8.orientation.z = 0.23904254249342105;
    pose8.orientation.w = 0.9583248133052252;
    waypoints.push_back(pose8);

    geometry_msgs::msg::Pose pose9;
    pose9.position.x = 0.09048433218830905;
    pose9.position.y = 0.11233859740087646;
    pose9.position.z = 0.15069511834155236;
    pose9.orientation.x = -0.006308112047193989;
    pose9.orientation.y = 0.025320410227902945;
    pose9.orientation.z = 0.24191090140946767;
    pose9.orientation.w = 0.9699475245226156;
    waypoints.push_back(pose9);

    geometry_msgs::msg::Pose pose10;
    pose10.position.x = 0.07891347645340688;
    pose10.position.y = 0.10620928378452835;
    pose10.position.z = 0.15352159805648152;
    pose10.orientation.x = -0.006335726731139758;
    pose10.orientation.y = 0.02542771707055573;
    pose10.orientation.z = 0.24195449779523703;
    pose10.orientation.w = 0.9699336630759976;
    waypoints.push_back(pose10);

    geometry_msgs::msg::Pose pose11;
    pose11.position.x = 0.05341474564016524;
    pose11.position.y = 0.09252098507666111;
    pose11.position.z = 0.1455771372769992;
    pose11.orientation.x = -0.014774327731087568;
    pose11.orientation.y = 0.05923164232719785;
    pose11.orientation.z = 0.24125709998639794;
    pose11.orientation.w = 0.9685392834023154;
    waypoints.push_back(pose11);

    geometry_msgs::msg::Pose pose12;
    pose12.position.x = 0.06785862276609957;
    pose12.position.y = 0.04701154110675846;
    pose12.position.z = 0.15585413287456126;
    pose12.orientation.x = -0.004247732537266147;
    pose12.orientation.y = 0.03460675290044033;
    pose12.orientation.z = 0.12175632173110766;
    pose12.orientation.w = 0.9919474419244655;
    waypoints.push_back(pose12);

    geometry_msgs::msg::Pose pose13;
    pose13.position.x = 0.3035704582748756;
    pose13.position.y = -4.6019088696090045e-05;
    pose13.position.z = 0.30536264212765296;
    pose13.orientation.x = 1.8345570569793093e-06;
    pose13.orientation.y = -7.180863568924419e-05;
    pose13.orientation.z = -6.589662785214818e-05;
    pose13.orientation.w = 0.9999999952488945;
    waypoints.push_back(pose13);
/* #endregion */

    move_group.setStartStateToCurrentState();
    moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
    double fraction = move_group.computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);
    
    RCLCPP_INFO(logger, "Trajectory %.2f%% achieved", fraction * 100.0);
    
    if (fraction >= 0.8) {
        scale_trajectory_speed(cartesian_plan, 1.4);
        move_group.execute(cartesian_plan);
        RCLCPP_INFO(logger, "Trajectory execution completed");
        return true;
    } else {
        RCLCPP_ERROR(logger, "Failed to plan Cartesian path sufficiently");
        return false;
    }
}

bool executeCartesianPath2(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger) {
    if (!returnToMiddle(move_group, logger)) {
        RCLCPP_ERROR(logger, "Failed to return to middle position before executing Cartesian path.");
        return false;
    }
    RCLCPP_INFO(logger, "Starting Cartesian path execution");

    /* #region  */
    std::vector<geometry_msgs::msg::Pose> waypoints;
    // 目标点1 0/-50/90/15/0
    geometry_msgs::msg::Pose pose1;
    pose1.position.x = -0.011801240508450508;
    pose1.position.y = -1.6717636845861036e-05;
    pose1.position.z = 0.14428133950504773;
    pose1.orientation.x = -2.3927732160852933e-06;
    pose1.orientation.y = 0.3826546744537482;
    pose1.orientation.z = -5.627848297886214e-05;
    pose1.orientation.w = 0.9238914421866365;
    waypoints.push_back(pose1);

    // 目标点2 0/-70/120/-12/0
    geometry_msgs::msg::Pose pose2;
    pose2.position.x = 0.02270275459045043;
    pose2.position.y = -3.0206178897082266e-05;
    pose2.position.z = 0.08515341454770431;
    pose2.orientation.x = -3.0504780108955957e-05;
    pose2.orientation.y = 0.32557952141613367;
    pose2.orientation.z = -8.924993019861189e-05;
    pose2.orientation.w = 0.9455146568606697;
    waypoints.push_back(pose2);

    // 目标点3 0/-65/121/-18/0
    geometry_msgs::msg::Pose pose3;
    pose3.position.x = 0.03716382693672583;
    pose3.position.y = -1.5665732298609626e-05;
    pose3.position.z = 0.08467326062547971;
    pose3.orientation.x = -2.6268154351170503e-05;
    pose3.orientation.y = 0.3256253211597455;
    pose3.orientation.z = -4.6484770819418694e-05;
    pose3.orientation.w = 0.945498888084361;
    waypoints.push_back(pose3);

    // 目标点4 0/-68/128/-27/0
    geometry_msgs::msg::Pose pose4;
    pose4.position.x = 0.05279465803518045;
    pose4.position.y = 2.85229202356785e-05;
    pose4.position.z = 0.08403403724734092;
    pose4.orientation.x = -1.0133215588657257e-05;
    pose4.orientation.y = 0.2839328542037646;
    pose4.orientation.z = 7.950130493131293e-05;
    pose4.orientation.w = 0.9588441624584073;
    waypoints.push_back(pose4);

    // 目标点5 0/-70/135/-33/0 cz 0/-73/135/-34/0
    geometry_msgs::msg::Pose pose5;
    pose5.position.x = 0.06279208075096127;
    pose5.position.y = 8.436405029428977e-06;
    pose5.position.z = 0.08359961953583876;
    pose5.orientation.x = -1.421938048450076e-05;
    pose5.orientation.y = 0.2418572307363304;
    pose5.orientation.z = 1.8897337572279438e-05;
    pose5.orientation.w = 0.9703118464603292;
    waypoints.push_back(pose5);

    // 目标点6 0/-70/141/-50/0
    geometry_msgs::msg::Pose pose6;
    pose6.position.x = 0.08872055239306093;
    pose6.position.y = 0.00012314349764190854;
    pose6.position.z = 0.09403262420395148;
    pose6.orientation.x = -1.2451783763174253e-05;
    pose6.orientation.y = 0.1823097069171027;
    pose6.orientation.z = 0.0003162268136817005;
    pose6.orientation.w = 0.9832411050242742;
    waypoints.push_back(pose6);

    // 目标点7 0/-72/150/-78/0 举平 cz 0/-72/155/-85/0
    geometry_msgs::msg::Pose pose7;
    pose7.position.x = 0.1175734673974146;
    pose7.position.y = 1.2029243031720646e-05;
    pose7.position.z = 0.14823841510901875;
    pose7.orientation.x = 1.4323529733784588e-06;
    pose7.orientation.y = -0.01745669931009907;
    pose7.orientation.z = 3.443876281589146e-05;
    pose7.orientation.w = 0.9998476196206685;
    waypoints.push_back(pose7);

    //第10个目标位姿 0/-75/145/-67/0 guaban
    geometry_msgs::msg::Pose pose10;
    pose10.position.x = 0.0764600578178136;
    pose10.position.y = -1.2478925669281682e-05;
    pose10.position.z = 0.14562989389494255;
    pose10.orientation.x = -1.1462464329986475e-05;
    pose10.orientation.y = 0.06097964734858765;
    pose10.orientation.z = -5.709821832855558e-05;
    pose10.orientation.w = 0.998139007963143;
    waypoints.push_back(pose10);

    // //0/-90/140/-46/0 刮板出来
    // //0/-87/140/-46/0 刮板出来
    // paht2不需要是因为本身有一次刮板动作
    // geometry_msgs::msg::Pose pose11;
    // pose11.position.x = 0.10539415348450036;
    // pose11.position.y =  -2.546318056005476e-05;
    // pose11.position.z = 0.15357808648518967;
    // pose11.orientation.x = -2.821210798315425e-06;
    // pose11.orientation.y = 0.026120173436386493;
    // pose11.orientation.z = -9.598835811991282e-05;
    // pose11.orientation.w = 0.9996588054521047;
    // waypoints.push_back(pose11);

    //14/-90/140/-46/0 刮板出来平移
    geometry_msgs::msg::Pose pose12;
    pose12.position.x = 0.06784517531798526;
    pose12.position.y = -0.04705705723709438;
    pose12.position.z = 0.1558304933958344;
    pose12.orientation.x = 0.004249768597968782;
    pose12.orientation.y = 0.0346403265165309;
    pose12.orientation.z = -0.12190441708021359;
    pose12.orientation.w = 0.9919280721614977;
    waypoints.push_back(pose12);

    geometry_msgs::msg::Pose pose13;
    pose13.position.x = 0.3035704582748756;
    pose13.position.y = -4.6019088696090045e-05;
    pose13.position.z = 0.30536264212765296;
    pose13.orientation.x = 1.8345570569793093e-06;
    pose13.orientation.y = -7.180863568924419e-05;
    pose13.orientation.z = -6.589662785214818e-05;
    pose13.orientation.w = 0.9999999952488945;
    waypoints.push_back(pose13);
/* #endregion */

    move_group.setStartStateToCurrentState();
    moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
    double fraction = move_group.computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);
    
    RCLCPP_INFO(logger, "Trajectory %.2f%% achieved", fraction * 100.0);
    
    if (fraction >= 0.8) {
        scale_trajectory_speed(cartesian_plan, 1.4);
        move_group.execute(cartesian_plan);
        RCLCPP_INFO(logger, "Trajectory execution completed");
        return true;
    } else {
        RCLCPP_ERROR(logger, "Failed to plan Cartesian path sufficiently");
        return false;
    }
}

bool executeCartesianPath3(moveit::planning_interface::MoveGroupInterface &move_group, rclcpp::Logger logger) {
    if (!returnToMiddle(move_group, logger)) {
        RCLCPP_ERROR(logger, "Failed to return to middle position before executing Cartesian path.");
        return false;
    }
    RCLCPP_INFO(logger, "Starting Cartesian path execution");

    /* #region  */
    std::vector<geometry_msgs::msg::Pose> waypoints;
    // 目标点1 -14/-45/90/-45/0
    geometry_msgs::msg::Pose pose1;
    pose1.position.x = 0.1038794732882471;
    pose1.position.y = -0.055993214192435026;
    pose1.position.z = 0.2876041751758597;
    pose1.orientation.x = 7.895216365982998e-06;
    pose1.orientation.y = 4.925607556231088e-05;
    pose1.orientation.z = -0.12184804269604492;
    pose1.orientation.w = 0.9925487655539387;
    waypoints.push_back(pose1);

    // 目标点2 -28/-45/90/-45/0
    geometry_msgs::msg::Pose pose2;
    pose2.position.x = 0.08367758643029065;
    pose2.position.y = -0.1086808498137246;
    pose2.position.z = 0.2875887574546225;
    pose2.orientation.x = 1.927121538709558e-05;
    pose2.orientation.y = 6.971049619118313e-05;
    pose2.orientation.z = -0.24189446329796155;
    pose2.orientation.w = 0.97030256280959;
    waypoints.push_back(pose2);

    // 目标点3 -28/-50/80/15/0
    geometry_msgs::msg::Pose pose3;
    pose3.position.x = -0.024548290170542997;
    pose3.position.y = -0.051114431259759716;
    pose3.position.z = 0.14426120700169354;
    pose3.orientation.x = 0.09259218221194593;
    pose3.orientation.y = 0.37132642847766817;
    pose3.orientation.z = -0.223401705139517;
    pose3.orientation.w = 0.8964569423279657;
    waypoints.push_back(pose3);

    // 目标点4 -28/-70/120/-12/0
    geometry_msgs::msg::Pose pose4;
    pose4.position.x = 0.0059175229032937815;
    pose4.position.y = -0.06733821725529181;
    pose4.position.z = 0.08516913944763507;
    pose4.orientation.x = 0.07877489671301727;
    pose4.orientation.y = 0.3158618841182072;
    pose4.orientation.z = -0.2287160122165187;
    pose4.orientation.w = 0.9174501466373653;
    waypoints.push_back(pose4);

    // 目标点5 -28/-65/121/-17/0
    geometry_msgs::msg::Pose pose5;
    pose5.position.x = 0.014856622895562388;
    pose5.position.y = -0.0721048590648025;
    pose5.position.z = 0.06944823076037311;
    pose5.orientation.x = 0.08474302001326812;
    pose5.orientation.y = 0.3397336582584099;
    pose5.orientation.z = -0.22661422321503966;
    pose5.orientation.w = 0.908870538548826;
    waypoints.push_back(pose5);

    // 目标点6 -28/-68/128/-27/0
    geometry_msgs::msg::Pose pose6;
    pose6.position.x = 0.032492179697122675;
    pose6.position.y = -0.0814728216582047;
    pose6.position.z = 0.08401846064911692;
    pose6.orientation.x = 0.06864384957945104;
    pose6.orientation.y = 0.27555892205004473;
    pose6.orientation.z = -0.23195179933670168;
    pose6.orientation.w = 0.9303513665158974;
    waypoints.push_back(pose6);

    // 目标点7 -28/-70/135/-36/0
    geometry_msgs::msg::Pose pose7;
    pose7.position.x = 0.0446287643769399;
    pose7.position.y = -0.08791412462470531;
    pose7.position.z = 0.08061218194929645;
    pose7.orientation.x = 0.06051325339911822;
    pose7.orientation.y = 0.242909963020376;
    pose7.orientation.z = -0.23418814928531106;
    pose7.orientation.w = 0.9394087538248804;
    waypoints.push_back(pose7);

    // 目标点8 -28/-70/141/-50/0
    geometry_msgs::msg::Pose pose8;
    pose8.position.x = 0.08054424192783191;
    pose8.position.y = -0.10699752705986922;
    pose8.position.z = 0.10661899188484153;
    pose8.orientation.x = 0.03779276087929698;
    pose8.orientation.y = 0.15177439309731033;
    pose8.orientation.z = -0.2389260449080508;
    pose8.orientation.w = 0.9583582763714517;
    waypoints.push_back(pose8);

    // 目标点9 -28/-72/150/-78/0 举平
    geometry_msgs::msg::Pose pose9;
    pose9.position.x = 0.09049845893474838;
    pose9.position.y = -0.11230423464398587;
    pose9.position.z = 0.1506514470154047;
    pose9.orientation.x = 0.006333520542832988;
    pose9.orientation.y = 0.025396788065378907;
    pose9.orientation.z = -0.24180305139626612;
    pose9.orientation.w = 0.969972254246971;

    //第10个目标位姿 28/-75/145/-67/0 guaban
    geometry_msgs::msg::Pose pose10;
    pose10.position.x = 0.07891456710427466;
    pose10.position.y = -0.10620567254945851;
    pose10.position.z = 0.1535640697602633;
    pose10.orientation.x = 0.006326862337248406;
    pose10.orientation.y = 0.025368477442986332;
    pose10.orientation.z = -0.24193326476205457;
    pose10.orientation.w = 0.969940568574572;
    waypoints.push_back(pose10);

    //28/-90/140/-46/0 刮板出来
    //28/-84/140/-46/0 刮板出来
    geometry_msgs::msg::Pose pose11;
    pose11.position.x = 0.05443307069880887;
    pose11.position.y = -0.09315578513527038;
    pose11.position.z = 0.13130145519485553;
    pose11.orientation.x = 0.023189180303617867;
    pose11.orientation.y = 0.09297061544632794;
    pose11.orientation.z = -0.24082394279570615;
    pose11.orientation.w = 0.965827394080696;
    waypoints.push_back(pose11);

    //28/-90/140/-46/0 刮板出来平移
    geometry_msgs::msg::Pose pose12;
    pose12.position.x = 0.06784517531798526;
    pose12.position.y = -0.04705705723709438;
    pose12.position.z = 0.1558304933958344;
    pose12.orientation.x = 0.004249768597968782;
    pose12.orientation.y = 0.0346403265165309;
    pose12.orientation.z = -0.12190441708021359;
    pose12.orientation.w = 0.9919280721614977;
    waypoints.push_back(pose12);

    geometry_msgs::msg::Pose pose13;
    pose13.position.x = 0.3035704582748756;
    pose13.position.y = -4.6019088696090045e-05;
    pose13.position.z = 0.30536264212765296;
    pose13.orientation.x = 1.8345570569793093e-06;
    pose13.orientation.y = -7.180863568924419e-05;
    pose13.orientation.z = -6.589662785214818e-05;
    pose13.orientation.w = 0.9999999952488945;
    waypoints.push_back(pose13);
    /* #endregion */

    move_group.setStartStateToCurrentState();
    moveit::planning_interface::MoveGroupInterface::Plan cartesian_plan;
    double fraction = move_group.computeCartesianPath(waypoints, 0.01, 0.0, cartesian_plan.trajectory_);
    
    RCLCPP_INFO(logger, "Trajectory %.2f%% achieved", fraction * 100.0);
    
    if (fraction >= 0.8) {
        scale_trajectory_speed(cartesian_plan, 1.4);
        move_group.execute(cartesian_plan);
        RCLCPP_INFO(logger, "Trajectory execution completed");
        return true;
    } else {
        RCLCPP_ERROR(logger, "Failed to plan Cartesian path sufficiently");
        return false;
    }
}

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto moveit_node = rclcpp::Node::make_shared("zc4_moveit_control");
    moveit::planning_interface::MoveGroupInterface move_group(moveit_node, "zc4");
    
    bool success = executeCartesianPath2(move_group, moveit_node->get_logger());
    
    rclcpp::shutdown();
    return 0;
}